/*
 * @Author: Mcfly
 * @Date: 2021-07-18 21:29:39
 * @LastEditTime: 2021-11-07 05:46:48
 * @LastEditors: Mcfly
 * @Description: 
 * @FilePath: \FTTfly\BSPdriver\OpenMV.c
 * 本人github:https://github.com/McflyWZX
 */
#include "OpenMV.h"
#include "stdlib.h"
#include "string.h"
#include "Usart.h"
#include "uart.h"

#define HuiMV_UsartInit() ConfigureUART3()
#define HuiMV_UsartSend(buf, len) USART3_Send(buf, len)
#define HuiMV_UsartRecv(buf, len) USART3_Get(buf, len, 9999) //int32

#define EyeMV_UsartInit() ConfigureUART0()
#define EyeMV_UsartSend(buf, len) USART0_Send(buf, len)
#define EyeMV_UsartRecv(buf, len) USART0_Get(buf, len, 9999) //int32

HuiMV *HuiMV_Init(void)
{
    HuiMV_UsartInit();
    HuiMV *mv = (HuiMV *)malloc(sizeof(HuiMV));
    memset(mv, 0x00, sizeof(HuiMV));
    return mv;
}

HuiMV_RawData HuiMV_DataRecv;
uint8_t HuiMV_ReqPack = 0xaa;
void HuiMV_Req(HuiMV *mv)
{
    uint8_t recvOK = 0;
    uint8_t tryTimes = 0;
	uint16_t dataLen = &HuiMV_DataRecv.checkSum - (uint8_t*)&HuiMV_DataRecv.x + 1;
	mv->dataLoop = 1;
	mv->dataOK = 2;
    do
    {
        while (UARTCharGetNonBlocking(UART3_BASE) != -1)
        {
        };
        HuiMV_UsartSend(&HuiMV_ReqPack, 1);
        if (!HuiMV_UsartRecv((uint8_t *)&HuiMV_DataRecv, dataLen))
        {
            recvOK = 0;
            continue;
        }
        else
        {
            uint8_t checkSum = 0x00;
            uint8_t *bufStart = (uint8_t *)&HuiMV_DataRecv;
            for (int i = 0; i < dataLen - 1; i++)
            {
                checkSum += bufStart[i];
            }
            if (checkSum != HuiMV_DataRecv.checkSum)
            {
                recvOK = 0;
                continue;
            }
            else
            {
                recvOK = 1;
                mv->x = 0.55f * HuiMV_DataRecv.x + 0.45f * mv->x;
				mv->y = 0.55f * HuiMV_DataRecv.y + 0.45f * mv->y;
                mv->angle = 0.55f * HuiMV_DataRecv.angle / 10.0f + 0.45f * mv->angle;
                mv->option = HuiMV_DataRecv.option;
				mv->dataOK = 1;
                return;
            }
        }
    } while (++tryTimes < 5 && !recvOK);
	mv->dataOK = 0;
}

void HuiMV_SetFun(HuiMV *mv, uint8_t reqPack)
{
    HuiMV_ReqPack = reqPack;
}

EyeMV *EyeMV_Init(void)
{
    EyeMV_UsartInit();
    EyeMV *mv = (EyeMV *)malloc(sizeof(EyeMV));
    memset(mv, 0x00, sizeof(EyeMV));
    return mv;
}

EyeMV_RawData EyeMV_DataRecv;
uint8_t EyeMV_ReqPack = 0xaa;
uint8_t superOption = 0;
void EyeMV_Req(EyeMV *mv)
{
    //当有特殊功能请求的时候，此轮改为特殊功能包，并在程序结束之后还原
    if(superOption)
    {
        EyeMV_ReqPack = superOption;
    }

    uint8_t recvOK = 0;
    uint8_t tryTimes = 0;
	uint16_t dataLen = &EyeMV_DataRecv.checkSum - (uint8_t*)&EyeMV_DataRecv.midErr + 1;
	mv->dataLoop = 1;
	mv->dataOK = 2;
    do
    {
        while (UARTCharGetNonBlocking(UART0_BASE) != -1)
        {
        };
        EyeMV_UsartSend(&EyeMV_ReqPack, 1);
        if (!EyeMV_UsartRecv((uint8_t *)&EyeMV_DataRecv, dataLen))
        {
            recvOK = 0;
            continue;
        }
        else
        {
            uint8_t checkSum = 0x00;
            uint8_t *bufStart = (uint8_t *)&EyeMV_DataRecv;
            //计算校验和
            for (int i = 0; i < dataLen - 1; i++)
            {
                checkSum += bufStart[i];
            }
            //如果校验和出问题，有可能是真的出问题，也有可能是没找到目标
            if (checkSum != EyeMV_DataRecv.checkSum)
            {
                //如果是没找到目标，另行处理
                if( EyeMV_DataRecv.midErr == 50 && EyeMV_DataRecv.distance == 100 && EyeMV_DataRecv.checkSum == 1)
                {
                    recvOK = 1;
                    mv->dataOK = 1;
                    if(mv->lossCount < 12)mv->lossCount++;
                    return;
                }
                recvOK = 0;
                continue;
            }
            else
            {
                if(EyeMV_DataRecv.distance == 20)
                {
                    recvOK = 1;
                    mv->midErr = 0.48f * EyeMV_DataRecv.midErr + 0.52f * mv->midErr;
                    mv->distance = 0.48f * EyeMV_DataRecv.distance + 0.52f * mv->distance;
                    mv->targetFund = 1;
                    mv->dataOK = 1;
                    if(mv->lossCount < 12)mv->lossCount++;
                } else {
                    recvOK = 1;
                    mv->midErr = 0.48f * EyeMV_DataRecv.midErr + 0.52f * mv->midErr;
                    mv->distance = 0.48f * EyeMV_DataRecv.distance + 0.52f * mv->distance;
                    mv->targetFund = 0;
                    mv->dataOK = 1;
                    mv->lossCount = 0;
                }
                superOption = 0;
                EyeMV_ReqPack = 0xaa;
                return;
            }
        }
    } while (++tryTimes < 2 && !recvOK);
	mv->dataOK = 0;
}

void EyeMV_SuperFun(EyeMV *mv, uint8_t reqPack)
{
    superOption = reqPack;
}
